#include "PowerFlowCalculation.h"

gsl_matrix* EqualHeisenMatrix(Node m_Node[],gsl_matrix* m_G,gsl_matrix *m_B,gsl_matrix* m_y,int Node_Num,int GenCount,int nEquals,int nVaribles)
{
    gsl_matrix* Hession_H=gsl_matrix_alloc(nVaribles,nVaribles);
    gsl_matrix* Hession=gsl_matrix_alloc(nVaribles,nVaribles);

//**************** ΔP部分 ***************************
 
    gsl_matrix* h1=gsl_matrix_alloc(GenCount,GenCount);
    gsl_matrix* h2=gsl_matrix_alloc(GenCount,Node_Num);
    gsl_matrix* h3=gsl_matrix_alloc(GenCount,Node_Num-1);
    gsl_matrix* h4=gsl_matrix_alloc(Node_Num,GenCount);
    gsl_matrix* h5=gsl_matrix_alloc(Node_Num,Node_Num);
    gsl_matrix* h6=gsl_matrix_alloc(Node_Num,Node_Num);
    gsl_matrix* h7=gsl_matrix_alloc(Node_Num-1,GenCount);
    gsl_matrix* h8=gsl_matrix_alloc(Node_Num-1,Node_Num);
    gsl_matrix* h9=gsl_matrix_alloc(Node_Num-1,Node_Num-1);
    gsl_matrix* h10=gsl_matrix_alloc(Node_Num,Node_Num-1);
    gsl_matrix* h11=gsl_matrix_alloc(Node_Num-1,Node_Num-1);

    

    

    for(int i=0;i<Node_Num-1;i++)
    {
        for(int j=0;j<Node_Num;j++)
        {
            if(gsl_matrix_get(m_B,i,j)!=0)
            {
                if(j==i)
                {
                    gsl_matrix_set(h5,i,i,gsl_matrix_get(h5,i,i)-gsl_matrix_get(m_y,i,0)*2*gsl_matrix_get(m_G,i,i));
                    if(m_Node[j].Type!=2)
                    {
                        gsl_matrix_set(h6,i,i,gsl_matrix_get(h6,i,i)+gsl_matrix_get(m_y,i,0)*(m_Node[i].Vol*gsl_matrix_get(m_B,i,i)+m_Node[i].Q/m_Node[i].Vol));
                        if(m_Node[i].Type!=2)
                        {
                            gsl_matrix_set(h9,i,i,gsl_matrix_get(h9,i,i)+gsl_matrix_get(m_y,i,0)*(m_Node[i].P-m_Node[i].Vol*m_Node[i].Vol*gsl_matrix_get(m_G,i,i)));
                        }
                    }
                }
                else
                {

                    double f1 = m_Node[i].Angle-m_Node[j].Angle;
                    double f2 = gsl_matrix_get(m_G,i,j)*cos(f1);
                    double f3 = gsl_matrix_get(m_G,i,j)*sin(f1);
                    double f4 = gsl_matrix_get(m_B,i,j)*cos(f1);
                    double f5 = gsl_matrix_get(m_B,i,j)*sin(f1);
                    if(j<i)
                    {
                        gsl_matrix_set(h5,i,j,gsl_matrix_get(h5,i,j)-gsl_matrix_get(m_y,i,0)*(f2+f5));
                        gsl_matrix_set(h5,j,i,gsl_matrix_get(h5,i,j));
                        if(m_Node[j].Type != 2)
                        {
                            gsl_matrix_set(h6,i,j,gsl_matrix_get(h6,i,j)-gsl_matrix_get(m_y,i,0)*m_Node[j].Vol*(f3-f4));
                            gsl_matrix_set(h6,j,i,gsl_matrix_get(h6,j,i)+gsl_matrix_get(m_y,i,0)*m_Node[i].Vol*(f3-f4));
                            gsl_matrix_set(h6,j,j,gsl_matrix_get(h6,j,j)-gsl_matrix_get(m_y,i,0)*m_Node[i].Vol*(f3-f4));
                            gsl_matrix_set(h9,i,j,gsl_matrix_get(h9,i,j)-gsl_matrix_get(m_y,i,0)*m_Node[i].Vol*m_Node[j].Vol*(f2+f5));
                            gsl_matrix_set(h9,j,i,gsl_matrix_get(h9,j,i)-gsl_matrix_get(m_y,i,0)*m_Node[i].Vol*m_Node[j].Vol*(f2+f5));
                            gsl_matrix_set(h9,j,j,gsl_matrix_get(h9,j,j)+gsl_matrix_get(m_y,i,0)*m_Node[i].Vol*m_Node[j].Vol*(f2+f5));
                        }
                    }
                    else
                    {
                        gsl_matrix_set(h5,i,j,-gsl_matrix_get(m_y,i,0)*(f2+f5));
                        gsl_matrix_set(h5,j,i,gsl_matrix_get(h5,i,j));
                        gsl_matrix_set(h6,i,j,-gsl_matrix_get(m_y,i,0)*m_Node[j].Vol*(f3-f4));
                        gsl_matrix_set(h6,j,i,gsl_matrix_get(m_y,i,0)*m_Node[i].Vol*(f3-f4));
                        gsl_matrix_set(h6,j,j,gsl_matrix_get(h6,j,j)-gsl_matrix_get(m_y,i,0)*m_Node[i].Vol*(f3-f4));
                        if((m_Node[j].Type!=2) && (m_Node[i].Type!=2))
                        {
                        gsl_matrix_set(h9,i,j,-gsl_matrix_get(m_y,i,0)*m_Node[i].Vol*m_Node[j].Vol*(f2+f5));
                        gsl_matrix_set(h9,j,i,-gsl_matrix_get(m_y,i,0)*m_Node[i].Vol*m_Node[j].Vol*(f2+f5));
                        gsl_matrix_set(h9,j,j,gsl_matrix_get(h9,j,j)+gsl_matrix_get(m_y,i,0)*m_Node[i].Vol*m_Node[j].Vol*(f2+f5));
                        }
                    }
                }
            }
        }
    }

    


    for(int i=0;i<Node_Num;i++)
    {
        for(int j=0;j<Node_Num-1;j++)
        {
            gsl_matrix_set(h10,i,j,gsl_matrix_get(h6,i,j));
        }
    }
    
    gsl_matrix_free(h6);
    h6=gsl_matrix_alloc(Node_Num,Node_Num-1);
    gsl_matrix_memcpy(h6,h10);
    gsl_matrix_transpose_memcpy(h8,h6);

    for(int i=0;i<GenCount;i++)
    {
        for(int j=0;j<GenCount;j++)
        {
            gsl_matrix_set(Hession,i,j,gsl_matrix_get(h1,i,j));
        }
    }
    for(int i=0;i<GenCount;i++)
    {
        for(int j=0;j<Node_Num;j++)
        {
            gsl_matrix_set(Hession,i,GenCount+j,gsl_matrix_get(h2,i,j));
        }
    }
    for(int i=0;i<GenCount;i++)
    {
        for(int j=0;j<Node_Num-1;j++)
        {
            gsl_matrix_set(Hession,i,GenCount+Node_Num+j,gsl_matrix_get(h3,i,j));
        }
    }
    for(int i=0;i<Node_Num;i++)
    {
        for(int j=0;j<GenCount;j++)
        {
            gsl_matrix_set(Hession,GenCount+i,j,gsl_matrix_get(h4,i,j));
        }
    }
    for(int i=0;i<Node_Num;i++)
    {
        for(int j=0;j<Node_Num;j++)
        {
            gsl_matrix_set(Hession,GenCount+i,j+GenCount,gsl_matrix_get(h5,i,j));
        }
    }
    for(int i=0;i<Node_Num;i++)
    {
        for(int j=0;j<Node_Num-1;j++)
        {
            gsl_matrix_set(Hession,GenCount+i,j+GenCount+Node_Num,gsl_matrix_get(h6,i,j));
        }
    }
    for(int i=0;i<Node_Num-1;i++)
    {
        for(int j=0;j<GenCount;j++)
        {
            gsl_matrix_set(Hession,GenCount+Node_Num+i,j,gsl_matrix_get(h7,i,j));
        }
    }
    for(int i=0;i<Node_Num-1;i++)
    {
        for(int j=0;j<Node_Num;j++)
        {
            gsl_matrix_set(Hession,GenCount+Node_Num+i,j+GenCount,gsl_matrix_get(h8,i,j));
        }
    }
    for(int i=0;i<Node_Num-1;i++)
    {
        for(int j=0;j<Node_Num-1;j++)
        {
            gsl_matrix_set(Hession,GenCount+Node_Num+i,j+GenCount+Node_Num,gsl_matrix_get(h9,i,j));
        }
    }
    for(int i=0;i<nVaribles;i++)
    {
        for(int j=0;j<nVaribles;j++)
        {
            gsl_matrix_set(Hession_H,i,j,gsl_matrix_get(Hession,i,j)+gsl_matrix_get(Hession_H,i,j));
        }
    }
    
    

    gsl_matrix_free(h1);
    gsl_matrix_free(h2);
    gsl_matrix_free(h3);
    gsl_matrix_free(h4);
    gsl_matrix_free(h5);
    gsl_matrix_free(h6);
    gsl_matrix_free(h7);
    gsl_matrix_free(h8);
    gsl_matrix_free(h9);
    gsl_matrix_free(h10);

    //    PRINT_MATRIX(Hession_H);
    // return Hession_H;

    
//**************** ΔQ部分 ***************************
    h1=gsl_matrix_alloc(GenCount,GenCount);
    h2=gsl_matrix_alloc(GenCount,Node_Num);
    h3=gsl_matrix_alloc(GenCount,Node_Num-1);
    h4=gsl_matrix_alloc(Node_Num,GenCount);
    h5=gsl_matrix_alloc(Node_Num,Node_Num);
    h6=gsl_matrix_alloc(Node_Num,Node_Num);
    h7=gsl_matrix_alloc(Node_Num-1,GenCount);
    h8=gsl_matrix_alloc(Node_Num-1,Node_Num);
    h9=gsl_matrix_alloc(Node_Num,Node_Num);
    h10=gsl_matrix_alloc(Node_Num,Node_Num-1);

    //printf("nodenum=%d\n",Node_Num);


    for(int i=0;i<Node_Num;i++)
    {
        for(int j=0;j<Node_Num;j++)
        {
            if(gsl_matrix_get(m_B,i,j)!=0)
            {
                if(j==i)
                {
                    gsl_matrix_set(h5,i,i,gsl_matrix_get(h5,i,i)+gsl_matrix_get(m_y,i+Node_Num-1,0)*2*gsl_matrix_get(m_B,i,i));
                    if(m_Node[j].Type!=2)
                    {
                        gsl_matrix_set(h6,i,i,gsl_matrix_get(h6,i,i)+gsl_matrix_get(m_y,i+Node_Num-1,0)*(m_Node[i].Vol*gsl_matrix_get(m_G,i,i)-m_Node[i].P/m_Node[i].Vol));
                        if(m_Node[i].Type!=2)
                        {
                            gsl_matrix_set(h9,i,i,gsl_matrix_get(h9,i,i)+gsl_matrix_get(m_y,i+Node_Num-1,0)*(m_Node[i].Vol*m_Node[i].Vol*gsl_matrix_get(m_B,i,i)+m_Node[i].Q));
                        }
                    }
                    
                }
                else
                {

                    double f1 = m_Node[i].Angle-m_Node[j].Angle;
                    double f2 = gsl_matrix_get(m_G,i,j)*cos(f1);
                    double f3 = gsl_matrix_get(m_G,i,j)*sin(f1);
                    double f4 = gsl_matrix_get(m_B,i,j)*cos(f1);
                    double f5 = gsl_matrix_get(m_B,i,j)*sin(f1);
                    if(j<i)
                    {
                        gsl_matrix_set(h5,i,j,gsl_matrix_get(h5,i,j)-gsl_matrix_get(m_y,i+Node_Num-1,0)*(f3-f4));
                        gsl_matrix_set(h5,j,i,gsl_matrix_get(h5,i,j));
                        if(m_Node[j].Type != 2)
                        {
                            gsl_matrix_set(h6,i,j,gsl_matrix_get(h6,i,j)+gsl_matrix_get(m_y,i+Node_Num-1,0)*m_Node[j].Vol*(f2+f5));
                            gsl_matrix_set(h6,j,i,gsl_matrix_get(h6,j,i)-gsl_matrix_get(m_y,i+Node_Num-1,0)*m_Node[i].Vol*(f2+f5));
                            gsl_matrix_set(h6,j,j,gsl_matrix_get(h6,j,j)+gsl_matrix_get(m_y,i+Node_Num-1,0)*m_Node[i].Vol*(f2+f5));
                            gsl_matrix_set(h9,i,j,gsl_matrix_get(h9,i,j)-gsl_matrix_get(m_y,i+Node_Num-1,0)*m_Node[i].Vol*m_Node[j].Vol*(f3-f4));
                            gsl_matrix_set(h9,j,i,gsl_matrix_get(h9,j,i)-gsl_matrix_get(m_y,i+Node_Num-1,0)*m_Node[i].Vol*m_Node[j].Vol*(f3-f4));
                            gsl_matrix_set(h9,j,j,gsl_matrix_get(h9,j,j)+gsl_matrix_get(m_y,i+Node_Num-1,0)*m_Node[i].Vol*m_Node[j].Vol*(f3-f4));
                        }
                    }
                    else
                    {
                        gsl_matrix_set(h5,i,j,-gsl_matrix_get(m_y,i+Node_Num-1,0)*(f3-f4));
                        gsl_matrix_set(h5,j,i,gsl_matrix_get(h5,i,j));
                        gsl_matrix_set(h6,i,j,gsl_matrix_get(m_y,i+Node_Num-1,0)*m_Node[j].Vol*(f2+f5));
                        gsl_matrix_set(h6,j,i,-gsl_matrix_get(m_y,i+Node_Num-1,0)*m_Node[i].Vol*(f2+f5));
                        gsl_matrix_set(h6,j,j,gsl_matrix_get(h6,j,j)+gsl_matrix_get(m_y,i+Node_Num-1,0)*m_Node[i].Vol*(f2+f5));
                        if((m_Node[j].Type!=2) && (m_Node[i].Type!=2))
                        {
                        gsl_matrix_set(h9,i,j,-gsl_matrix_get(m_y,i+Node_Num-1,0)*m_Node[i].Vol*m_Node[j].Vol*(f3-f4));
                        gsl_matrix_set(h9,j,i,-gsl_matrix_get(m_y,i+Node_Num-1,0)*m_Node[i].Vol*m_Node[j].Vol*(f3-f4));
                        gsl_matrix_set(h9,j,j,gsl_matrix_get(h9,j,j)+gsl_matrix_get(m_y,i+Node_Num-1,0)*m_Node[i].Vol*m_Node[j].Vol*(f3-f4));
                        }
                    }
                }
            }
        }
    }
   // PRINT_MATRIX(h5);

    for(int i=0;i<Node_Num;i++)
    {
        for(int j=0;j<Node_Num-1;j++)
        {
            gsl_matrix_set(h10,i,j,gsl_matrix_get(h6,i,j));
        }
    }
    gsl_matrix_free(h6);
    h6=gsl_matrix_alloc(Node_Num,Node_Num-1);
    
    gsl_matrix_memcpy(h6,h10);

    //printf("h6 size:w:%d,h:%d   h8 w:%d,h:%d\n",h6->size1,h6->size2,h8->size1,h8->size2);
    //return Hession_H; 
    gsl_matrix_transpose_memcpy(h8,h6);

     
    for(int i=0;i<Node_Num-1;i++)
    {
        for(int j=0;j<Node_Num-1;j++)
        {
            gsl_matrix_set(h11,i,j,gsl_matrix_get(h9,i,j));
        }
    }
    gsl_matrix_free(h9);
    h9=gsl_matrix_alloc(Node_Num-1,Node_Num-1);
    gsl_matrix_memcpy(h9,h11);


    for(int i=0;i<GenCount;i++)
    {
        for(int j=0;j<GenCount;j++)
        {
            gsl_matrix_set(Hession,i,j,gsl_matrix_get(h1,i,j));
        }
    }
    for(int i=0;i<GenCount;i++)
    {
        for(int j=0;j<Node_Num;j++)
        {
            gsl_matrix_set(Hession,i,GenCount+j,gsl_matrix_get(h2,i,j));
        }
    }
    for(int i=0;i<GenCount;i++)
    {
        for(int j=0;j<Node_Num-1;j++)
        {
            gsl_matrix_set(Hession,i,GenCount+Node_Num+j,gsl_matrix_get(h3,i,j));
        }
    }
    for(int i=0;i<Node_Num;i++)
    {
        for(int j=0;j<GenCount;j++)
        {
            gsl_matrix_set(Hession,GenCount+i,j,gsl_matrix_get(h4,i,j));
        }
    }
    for(int i=0;i<Node_Num;i++)
    {
        for(int j=0;j<Node_Num;j++)
        {
            gsl_matrix_set(Hession,GenCount+i,j+GenCount,gsl_matrix_get(h5,i,j));
        }
    }
    for(int i=0;i<Node_Num;i++)
    {
        for(int j=0;j<Node_Num-1;j++)
        {
            gsl_matrix_set(Hession,GenCount+i,j+GenCount+Node_Num,gsl_matrix_get(h6,i,j));
        }
    }
    for(int i=0;i<Node_Num-1;i++)
    {
        for(int j=0;j<GenCount;j++)
        {
            gsl_matrix_set(Hession,GenCount+Node_Num+i,j,gsl_matrix_get(h7,i,j));
        }
    }
    for(int i=0;i<Node_Num-1;i++)
    {
        for(int j=0;j<Node_Num;j++)
        {
            gsl_matrix_set(Hession,GenCount+Node_Num+i,j+GenCount,gsl_matrix_get(h8,i,j));
        }
    }
    for(int i=0;i<Node_Num-1;i++)
    {
        for(int j=0;j<Node_Num-1;j++)
        {
            gsl_matrix_set(Hession,GenCount+Node_Num+i,j+GenCount+Node_Num,gsl_matrix_get(h9,i,j));
        }
    }
    for(int i=0;i<nVaribles;i++)
    {
        for(int j=0;j<nVaribles;j++)
        {
            gsl_matrix_set(Hession_H,i,j,gsl_matrix_get(Hession,i,j)+gsl_matrix_get(Hession_H,i,j));
        }
    }

    gsl_matrix_free(h1);
    gsl_matrix_free(h2);
    gsl_matrix_free(h3);
    gsl_matrix_free(h4);
    gsl_matrix_free(h5);
    gsl_matrix_free(h6);
    gsl_matrix_free(h7);
    gsl_matrix_free(h8);
    gsl_matrix_free(h9);

    gsl_matrix_free(h11);
    

    return Hession_H;

}
